
#include "SymmetricMatrix.h"


//  ###########   Symmetric Matrix methods  ######################


Matrix SymmetricMatrix::jacobi() 
{
	SymmetricMatrix A=(*this);
	SymmetricMatrix B=A;
	
	long row,col,garbage;
	double cosTheta,sinTheta,tan2Theta,cos2Theta,theta;
	
	
	while(1)
	{
		B.disp();
		cin >> garbage;
		if(B.isDiagonal())
		{
			for(long i=0;i<numRows();++i)
			{
				(*Eigenvalues)[i][0]= B[i][i];
				eigenvalueCounter++;
			}
			
			return *Eigenvalues;
		}
		
		A=B;
		A.findLargest(&row,&col);
		 
		if(Vector::nearlyEqual(A[row][row],A[col][col]))
		{
			//theta=PI/4;
			cosTheta=sinTheta=0.707106781;
			
		}
		else
		{
			tan2Theta=(2 * A[row][col])/(A[row][row]-A[col][col]);
			
			
			cos2Theta=1/(sqrt(1+tan2Theta*tan2Theta));
			
			cosTheta=sqrt(0.5 * (1+cos2Theta));
			sinTheta=0.5 * tan2Theta * cos2Theta / cosTheta;
			
			cout <<"cosTheta is : " <<cosTheta  <<"   and sinTheta is : " <<sinTheta <<"\n\n";
			
        }
		
		/*
		// This approach also works
		SquareMatrix R(numRows());
		for(long i=0;i<numRows();++i) R[i][i]=1;
		
		R[row][row]=R[col][col]=cosTheta;
		R[row][col]=sinTheta;
		R[col][row]=-1.0 * sinTheta;
		
		B=SymmetricMatrix((R.transepose() * A) * R);
		*/
		
				
		//these 2 elements will become 0
		B[row][col]= B[col][row]= (A[col][col]-A[row][row])*(sinTheta * cosTheta) + A[row][col] * (cosTheta * cosTheta - sinTheta * sinTheta);
		
		
		B[row][row]= A[row][row] * cosTheta * cosTheta + A[col][col] * sinTheta * sinTheta + 2 * A[row][col] * cosTheta * sinTheta;
		
		
		B[col][col]= A[row][row] * sinTheta * sinTheta + A[col][col] * cosTheta * cosTheta - 2 * A[row][col] * cosTheta * sinTheta;
		
		//cout <<"B[row][row]= " <<B[0][0];
		//fflush(stdout);
		
		
		//other elements - where one of the indexes matches either row or col
		for(int j=0;j<numCols();++j)
		{
		    if(j==row or j==col) continue;
			
			B[row][j] = B[j][row] = A[row][j] * cosTheta + A[col][j] * sinTheta;
			B[col][j] = B[j][col] = A[col][j] * cosTheta - A[row][j] * sinTheta;
			
		}
			
		
		/*	
		//ohter elements - in a bit roundabout manner
		for(long i=0;i<numRows();++i)
		{
			for(long j=0;j<i;++j)
			{
				if((i==row and j==col) or (i==col and j==row))
					continue;
					
				if(i==row or j==row)
				{
				    cout <<"\nchanging " <<i <<"  " <<j <<"\n";
					fflush(stdout);
					
					long other;
					if(i==row) other=j;
					else other=i;
					
					B[i][j]= B[j][i]= A[row][other] * cosTheta + A[col][other] * sinTheta;
				}
				
				if(i==col or j==col)
				{
				    cout <<"\nchanging " <<i <<"  " <<j <<"\n";
					fflush(stdout);
					
					long other;
					
					if(i==col) other=j;
					else other=i;
					
					B[i][j]= B[j][i]= A[col][other] * cosTheta - A[row][other] * sinTheta;
				}
				
			}
		} */
			
	}	
	
}


void SymmetricMatrix::findLargest(long* row, long* col) const
{
	*row=0;
	*col=1;
	
	for(long i=0; i<numRows(); ++i)
	{
		for(long j=0;j<numCols();++j)
		{
			if(i==j) continue;
			
			if(abs((*this)[i][j])>abs((*this)[*row][*col]))
			{
				*row=i;
				*col=j;
			}
		}
	}
}



Matrix SymmetricMatrix::givens() 
{
	
	SymmetricMatrix A=(*this);
	SymmetricMatrix B=A;
	
	long row,col,garbage;
	double cosTheta,sinTheta,tanTheta,cos2Theta,theta;
	
		
	for(long i=0; i<B.numRows(); ++i)
	{
		for(long j=i+2;j<B.numRows();++j)
		{
			//do not reduce tridiagonal elements
			//if((j==i) or (j==i-1) or (j==i+1)) continue;
			
			B.disp();			
			cin >> garbage;
			
			A=B;
			row=i;
			col=j;
			
			if(Vector::nearlyEqual(A[row][row+1],0.0))
			    tanTheta= tan(3.141592654/2);
			else
			    tanTheta= A[row][col]/A[row][row+1];
			
			cosTheta=sqrt(1/(1 + (tanTheta * tanTheta)));
			sinTheta=cosTheta * tanTheta;
		    
			cout <<"row " <<row <<"  col  " <<col <<"\n";
			cout <<"sinTheta is :  " <<sinTheta <<"  cosTheta is : " <<cosTheta <<"\n\n";
			
			
			//Matrix multiplication does not work for givens method
			/*
			//create I matrix
			SquareMatrix R(numRows(),0);
			for(long i=0;i<numRows();++i) R[i][i]=1;
			
			
			R[row+1][row+1]=R[col][col]=cosTheta;
			R[row+1][col]=sinTheta;
			R[col][row+1]=-1.0 * sinTheta;
			R.disp();
			R.transepose().disp();
			A=SymmetricMatrix((R.transepose() * A) * R);
			*/
			
			
			row=row+1;     //do the rotation in row+1,col plane
			//these 2 elements will become 0
		    B[row][col]= B[col][row]= (A[col][col]-A[row][row])*(sinTheta * cosTheta) + A[row][col] * (cosTheta * cosTheta - sinTheta * sinTheta);
		
		
		    B[row][row]= A[row][row] * cosTheta * cosTheta + A[col][col] * sinTheta * sinTheta + 2 * A[row][col] * cosTheta * sinTheta;
		
		
		    B[col][col]= A[row][row] * sinTheta * sinTheta + A[col][col] * cosTheta * cosTheta - 2 * A[row][col] * cosTheta * sinTheta;
		
		    //cout <<"B[row][row]= " <<B[0][0];
		    //fflush(stdout);
		
		
			//other elements - where one of the indexes matches either row or col
			for(int j=0;j<numCols();++j)
			{
				if(j==row or j==col) continue;

				B[row][j] = B[j][row] = A[row][j] * cosTheta + A[col][j] * sinTheta;
				B[col][j] = B[j][col] = A[col][j] * cosTheta - A[row][j] * sinTheta;

			}

			row=row-1;
		
		}
	}
		
	B.disp();
	//cout <<"\ncalling strum()\n";
	//cin >>garbage;
	
	return strum(B); 
	
}




Matrix SymmetricMatrix::strum(const SymmetricMatrix& B) 
{

	double * alpha = new double[B.numRows()];
	double * beta = new double[B.numRows()-1];
		
	int garbage;
	
	// store alphas and betas
	for(long i=0;i<B.numRows();++i)
	{
		alpha[i] = B[i][i];
		
		if(i==numRows()-1)
			break;
			
		beta[i] = B[i][i+1];
	}
	
	double maxEigen,minEigen,sum;
	
	//use Gerchgorin to find range
	for(long i=0; i<B.numRows(); ++i)
	{
		sum=0.0;
		for(long j=0;j<B.numCols();++j)
		{
			if(i==j) continue;
		
			sum = sum + abs(B[i][j]);
		}
		
		double tempMax= abs(B[i][i]) + sum;
		double tempMin= abs(B[i][i]) - sum;
		
		if(i==0)
		{
			maxEigen = tempMax;
			minEigen = tempMin;
		}
		else
		{
			if(maxEigen<tempMax)
				maxEigen=tempMax;
				
			if(minEigen>tempMin)
				minEigen=tempMin;
		}
		
		//cout <<"max :  " <<maxEigen <<"  min : " <<minEigen <<"\n";
		//fflush(stdout);
		//cin >>garbage;
	}
		
	cout <<"  minEigen " <<minEigen <<"  maxEigen " <<maxEigen <<"\n\n";
	fflush(stdout);
	cin >>garbage;
	applyStrums(alpha,beta,minEigen,maxEigen,1.0);
	
	return *Eigenvalues;
	
}	



void SymmetricMatrix::applyStrums(double* alpha, double* beta, double start, double end, double step)
{
			
	double * f = new double[numRows()+1];	//the fn(lambda)
	long currentV=0,lastV=-1;				//the function V(lambda)
	bool changed=false;
	int garbage; 
	
	//for each value in the range
	for(double lambda=start;lambda<=end;lambda=lambda+step)
	{
		
		// is this really necessary ?	
		if(eigenvalueCounter==numRows())
					return;
		
		f[0]=1;
		f[1]= lambda-alpha[0];
		
		//apply the generic rule to calculate fn(lambda)s
		for(long i=2;i<=numRows();++i)
		{
			f[i]= (lambda-alpha[i-1]) * f[i-1] - (beta[i-2] * beta[i-2]) * f[i-2];
		}
	
		//calculate number of sign changes
		currentV=0;
		for(long i=0;i<numRows();++i)
		{
			if(f[i] * f[i+1] < 0.0)
				currentV++;
		}
			
		if(lastV==-1)	//first iteration
			lastV=currentV;
			
			
		if(lastV!=currentV)		//found an interval
		{
		 	changed=true;
			
			if(step<=TOLERANCE)	//good enough an approximation
			{
				(*Eigenvalues)[eigenvalueCounter][0]= (lambda + (lambda-step))/2;
				eigenvalueCounter++;

				cout <<"storing " <<(*Eigenvalues)[eigenvalueCounter-1][0] <<"  as eigenvalue no  " <<eigenvalueCounter-1 <<"\n";
				fflush(stdout);
				cin >>garbage;
            			
			}
			
			else    //get better approximation
			{
                cout <<"an eigenvalue is between " <<lambda-step  <<"  and  " <<lambda <<"\n";

				applyStrums(alpha,beta,lambda - step, lambda, step/10);
				
            }
			
		}
		
		lastV=currentV;
	}
		
	if(!changed)
	{
	    (*Eigenvalues)[eigenvalueCounter][0]= (start+end)/2;
		eigenvalueCounter++;

		cout <<"storing " <<(*Eigenvalues)[eigenvalueCounter-1][0] <<"  as eigenvalue no  " <<eigenvalueCounter-1 <<"\n";
		fflush(stdout);
		cin >>garbage;
    
	}
	
	return;

	
}



Matrix SymmetricMatrix::householder()
{
	SymmetricMatrix A= (*this);
	
	double* x= new double[A.numRows()];
	
	for(long i=0;i<A.numRows()-2;++i)
	{
			
		Matrix W(A.numRows(),1);
		
		//first i+1 elements are 0
		for(long t=0;t<=i;++t) W[t][0]= 0.0;
		
		double s=0.0;
		
		for(long j=i+1;j<A.numCols();++j)
		{
			s= s + A[i][j] * A[i][j];
		}
		
		s= sqrt(s);
		
		A.disp();
		cout <<"\ns is : " <<s <<"\n";
		fflush(stdout);
		int garbage;
		cin >>garbage;
		
		
		double sgn= A[i][i+1]/abs(A[i][i+1]);
		
		W[i+1][0]= sqrt(0.5 * (1+ (A[i][i+1] * sgn/s)));
		
		for(long t=i+2;t<A.numRows();++t)
		{
			W[t][0]= (A[i][t] * sgn)/(2 * s * W[i+1][0]);
		}
		
		//create I matrix
		Matrix I(A.numRows(),A.numCols());
		for(long t=0;t<A.numRows();++t) I[t][t]=1;
		
		Matrix P= I- (W * W.transepose()) * 2;
		
		A= SymmetricMatrix((P * A) * P);
	
	}
		
	A.disp();
	cout <<"calling strum()\n";
	fflush(stdout);
		
	return strum(A);

}



//############  End of function  definitions  ############################################


int main()
{
	SymmetricMatrix A("A7");
		
	A.givens().disp();
	
	return 0;
}

